function u_pd = u_vector(x, p)
a = getrobotparameter('a');
ndof = length(x)/2;
dq = x(ndof+1:end);

M = D_mat(x);
C = C_mat(x);
G = G_vec(x);

% Outputs
y_a1 = ya1_sca(x, p, a);
y_a2 = ya2_vec(x, p, a);
y_d1 = yd1_sca(x, p, a);
y_d2 = yd2_vec(x, p, a);

y1 = y_a1 - y_d1;
y2 = y_a2 - y_d2;

% Jacobians of Outputs
Dy_a2 = Dya2_mat(x, p, a);
Dy_d2 = Dyd2_mat(x, p, a);

Dy_2 = Dy_a2 - Dy_d2;

% Control Fields
vf    = [dq; M \ (-C*dq - G)];

% Second Lie Derivative
Lfy2 = Dy_2*vf;

kp = 30;
kd = 10;

u_pd = [kd*y1;
    -kp*y2(2) - kd*Lfy2(2);
    -kp*y2(4) - kd*Lfy2(4);
    -kp*y2(1) - kd*Lfy2(1);
    -kp*y2(3) - kd*Lfy2(3)];

end